/*
 * testMain.cpp
 *
 *  Created on: Aug 3, 2009
 *      Author: naveenoid
 */

//#include "./BoardSetup/TitechBoard.hpp"
#include "./Common/Common.hpp"
#include "./MainLoop/ControlLoop.hpp"
#include "./MainLoop/MonitoringLoop.hpp"
#include "./MainLoop/ActuatingLoop.hpp"

#include "./ToolKit/Log/Logger.hpp"
#include "./MainLoop/Loop.hpp"

#include "./Trajectory/Planner.hpp"

#include <ecl/threads.hpp>
#include <ecl/time.hpp>

#include <stdio.h>
//#include <octave/oct.h>
//#include <octave/octave.h>
//#include <octave/parse.h>

//#include

//using namespace std;

//using namespace Octopus::BoardSetup;
using namespace Octopus::Common;
using namespace Octopus::MainLoop;
using namespace ECL::Threads;
using ECL::Time::StopWatch;
using ECL::Time::TimeStamp;
using Octopus::ToolKit::Log::Logger;
using Octopus::Common::sWait;

using Octopus::Trajectory::Planner;

//using ECL::Time::

int main()
{
	Logger *logger = Logger::getInstance("testLog.dat");

//	Loop *loop = ControlLoop::getInstance(logger);
//	Loop *loop = MonitoringLoop::getInstance(logger);
	Loop *loop = ActuatingLoop::getInstance(logger);

	Planner *planner = Planner::getInstance(loop, logger);



	Thread loopThread(&Loop::start, *loop);
	Thread plannerThread(&Planner::start, *planner);


//	char ch;
	double inp;
//	wait(10);
	while(true)
	{
//		scanf("%c",&ch);
		scanf("%lf",&inp);

//		logger->writeValues(ctr++,inp);

		printf("\n\nInput %lfN Received\n\n\n", inp);
//		if (ch=='q');
		if (inp > 1260.00)
		{
			printf("Breaking the loop\n");
			break;
		}
		else if(inp<= (double)TENSION_SAFETY_LIMIT)
		{
//			loop->offsetForce = inp;
			planner->setFixedPoint(inp);
		}

	}

	planner->stop();
	loop->stop();

	planner->remove();
	loop->remove();
	logger->remove();

	sWait(1);


////	  octave_main (0, 0, 1);//this throws the octave message but can be avoided
//	  ColumnVector x(2), y(2);
//	  x(0) =1; x(1)=2;
//	  y(0) =10; y(1)=20;
//	  octave_value_list f_arg, f_ret;
//	  f_arg(0) = octave_value(x);
//	  f_arg(1) = octave_value(y);
//	  f_ret=feval("plot",f_arg,1);
//	  if (!error_state )
//	  {
//	       cout << "ok" <<endl;
//	       double out = f_ret(0).double_value();
//	   }
//	  else
//	    std::cout << "error\n";




	return 0;
}

void stopWatchTest(int ctrLim)
{
	int ctr = 0;
	StopWatch stopwatch;
	TimeStamp time;
	printf("Checking wait\n");
	long int loopTime = 250000;
	long int time1 = 0, time2 = 0, time3 = 0, timeDiff = 0, totTime = 0, time1Prev = 0;
	double avgTime;

	while(ctr++<ctrLim)
	{
		time = stopwatch.elapsed();
		time1 = time.usec();
		printf("Initial = %ld \t",time1);
//		wait(0.10);

		uWait(50000);

		time = stopwatch.elapsed();
		time2 = time.usec();
		printf("Final = %ld \t",time2);

		timeDiff = loopTime-(time2-time1);
		printf("Must wait for %ld uSec \n", timeDiff);

		uWait(timeDiff);

		time = stopwatch.elapsed();

		time3 = time.usec();

		// Job
		totTime += time3 - time1;
		avgTime = (double)totTime / (double)ctr;

		printf ("Time3 is %ld uSec and total loopTime is %ld uSec and Average LoopTime is %f\n", time3, totTime, avgTime);

		time1Prev = time1;

		stopwatch.restart();
	}
}

